//SolidWall2d class and some creation functions for struts/walls
void addFXParticle(Vector3d p, Vector3d plast, Vector3d c, float a){
  if (activeFXParticles >= numFXParticles){ return; }
  fxParticles[activeFXParticles].position.set(p);//.copy();
  fxParticles[activeFXParticles].positionLast.set(plast);//.copy();
  fxParticles[activeFXParticles].truePositionLast.set(plast);//.copy();
  fxParticles[activeFXParticles].myColor.set(c);//.copy();
  fxParticles[activeFXParticles].alphaValue = a;
  fxParticles[activeFXParticles].setAirDrag(5.86);
  activeFXParticles++;
  return;
}

void removeFXParticle(int i){
  //To remove, simply move the last one on the list over this one and decrement the number of active ones
  if (activeFXParticles <= i) { return; }
  activeFXParticles--;
  fxParticles[i].position.set(fxParticles[activeFXParticles].position);
  fxParticles[i].positionLast.set(fxParticles[activeFXParticles].positionLast);
  fxParticles[i].truePositionLast.set(fxParticles[activeFXParticles].truePositionLast);
  fxParticles[i].myColor.set(fxParticles[activeFXParticles].myColor);
  fxParticles[i].alphaValue = fxParticles[activeFXParticles].alphaValue;
}
float fxAlphaDecay = 750.0;

Vector3d fxBuff = new Vector3d(0,0,0);
void stepFXParticles(float dt){

  for (int i=0; i<activeFXParticles; i++){
    fxBuff.set(gravityForce);
    fxBuff.multiply(fxParticles[i].mass);
    fxParticles[i].forceAdd(fxBuff);
    fxParticles[i].timeStep(dt);
    fxParticles[i].alphaValue -= fxAlphaDecay*dt;
    if (fxParticles[i].alphaValue < 5) removeFXParticle(i);
  }
}

void drawFXParticles(){
//  println("DRAWING FX PARTICLES");
  for (int i=0; i<activeFXParticles; i++){
    fxParticles[i].draw();
  }
//  println("DONE WITH FX PARTICLES");
}

void burstFXParticles(int num, Vector3d sourceLoc, Vector3d direction, Vector3d c, float scatter, float a){
  for (int i=0; i<num; i++){
//    debugCounter++;
    Vector3d p = sourceLoc.copy();
    Vector3d randomVec = new Vector3d(10*scatter,0,0);
    float scatterSqr = scatter*scatter;
    while (randomVec.normSquared() > scatterSqr){
      randomVec.set(random(-scatter,scatter),random(-scatter,scatter),0);
    }
    p.add(randomVec);
    Vector3d plast = sourceLoc.minus(direction);
    Vector3d diff = p.minus(plast);
    p.add(diff);
    plast.add(diff);
    addFXParticle(p,plast,c,a);
  }
}

void strutList(ArrayList partList, ArrayList strutArray){
  if (partList.size() > 1){
    for (int i=1; i<partList.size(); i++){
      strutArray.add(new Strut((Particle)partList.get(i-1),(Particle)partList.get(i)));
    }
  }
}

void wallList(Vector3d[] myPoints, ArrayList wallList){
  if (myPoints.length > 0){
    for (int i=1; i<myPoints.length; i++){
      wallList.add(new SolidWall2d(myPoints[i-1],myPoints[i]));
    }
  }
}

void iceWallList(Vector3d[] myPoints, ArrayList wallList){
  if (myPoints.length > 0){
    for (int i=1; i<myPoints.length; i++){
      wallList.add(new SolidWall2d(myPoints[i-1],myPoints[i],0.0,0.0,new Vector3d(0.0,0.0,255.0)));
    }
  }
}

void wallBox(float x1, float y1, float x2, float y2, ArrayList wallList){
  wallList.add(new SolidWall2d(x1,y1,x1,y2));
  wallList.add(new SolidWall2d(x1,y2,x2,y2));
  wallList.add(new SolidWall2d(x2,y2,x2,y1));
  wallList.add(new SolidWall2d(x2,y1,x1,y1));
}
void wall(float x1, float y1, float x2, float y2, ArrayList wallList){
  wallList.add(new SolidWall2d(x1,y1,x2,y2));
}

int lastID = 0;
//Has exclusive access to Vector3d vars bufferE and bufferF
final class SolidWall2d{
  //Currently assumes z=constant
  public Vector3d wallA;
  public Vector3d wallB;
  public float restCoef; //0 is no bounce, 1 is perfect bounce
  public float fricCoef; //1 is sticky, 0 is slippery
  public Vector3d wallColor;
  public int idNum;
  private boolean skipFlag;
  private Vector3d origV;

  {
    restCoef = 0;
    fricCoef = .99;
    wallColor = new Vector3d(255,255,255);
    idNum = ++lastID;
    skipFlag = false;
  }

  public SolidWall2d(Vector3d a, Vector3d b){
    wallA = new Vector3d(a);
    wallB = new Vector3d(b);
  }

  public SolidWall2d(float x1, float y1, float x2, float y2){
    wallA = new Vector3d(x1,y1,0);
    wallB = new Vector3d(x2,y2,0);
  }

  public SolidWall2d(Vector3d a, Vector3d b, float _restCoef, float _fricCoef, Vector3d _wallColor){
    wallA = new Vector3d(a);
    wallB = new Vector3d(b);
    restCoef = _restCoef;
    fricCoef = _fricCoef;
    wallColor = _wallColor;
  }

  public void draw(Vector3d customColor){
    strokeWeight(3);
    float factor = 2*max(width,height);
    if ( (-cameraX < (min(wallA.x,wallB.x)-factor)) || (-cameraX > (max(wallA.x,wallB.x)+factor) ) ){return;}
    if ( (-cameraY < (min(wallA.y,wallB.y)-factor)) || (-cameraY > (max(wallA.y,wallB.y)+factor) ) ){return;}

    stroke(customColor.x,customColor.y,customColor.z);
    //strokeWeight(4);
    if (drawing3d){
      line(wallA.x,wallA.y,wallA.z,wallB.x,wallB.y,wallB.z);
    }
    else{
      line(wallA.x,wallA.y,wallB.x,wallB.y);
    }
    strokeWeight(1);
    noStroke();
  }

  public void draw(){
    if(skipFlag){skipFlag = false; return;}
    else{draw(wallColor);}
  }

  public void skipNextDraw(){
    skipFlag = true;
  }

  private boolean colliding(Particle particle, Vector3d collisionPoint){
//    debugCounter++;
    //formula from http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/
    //This is a line-cross test, which might not be the best one to use.

    float x1 = particle.position.x;
    float y1 = particle.position.y;
    float x2 = particle.truePositionLast.x;
    float y2 = particle.truePositionLast.y;
    float x3 = wallA.x; 
    float y3 = wallA.y;
    float x4 = wallB.x; 
    float y4 = wallB.y;

    float ua = ( (x4-x3)*(y1-y3)-(y4-y3)*(x1-x3) );
    float ub = ( (x2-x1)*(y1-y3)-(y2-y1)*(x1-x3) );
    float denom = (y4-y3)*(x2-x1) - (x4-x3)*(y2-y1);
    if (abs(denom)<.00001){      
//      println("Oh crap");
      return false;
    }
    ua /= denom; 
    ub /= denom;

    if ( (0<ua)&&(ua<1)&&(0<ub)&&(ub<1) ){
      collisionPoint.x = (x1+ua*(x2-x1));
      collisionPoint.y = (y1+ua*(y2-y1));
      collisionPoint.z = 0;
      return true;
    }

    return false;
  }
  
    /** NOTE:
  	During main simulation, use collideTest to handle wall collisions.  However, if you are iteratively
	solving constraints, collideTest should not be used more than once per step, preferably near the beginning of
	the simulation step.  Instead, use the following algorithm:
	loop{ //main sim. loop
		for each particle: collideTest(particle, collisionPt);
		for (i=0; i<constraintIters; i++){
			applyNonWallConstraintsToParticles();
			for each particle: projectCollide(particle, collisionPt);
		}
	}
	Note that within constraint loop, projectCollide is used in lieu of collideTest!!!
	If you use collideTest more than once per time step, you'll get pwned.  You're warned.
  */
  
  public boolean collideTest(Particle particle, Vector3d collisionPoint){
    //Now clean of Vector3d creation except for player particles

    if (particle.pinned) return false;
    
    float factor = 2.0*particle.radius;
    float maxX = max(particle.position.x,particle.truePositionLast.x);	
    float maxY = max(particle.position.y,particle.truePositionLast.y);
    float minX = min(particle.position.x,particle.truePositionLast.x);	
    float minY = min(particle.position.y,particle.truePositionLast.y);
    float leftX = min(wallA.x,wallB.x); float rightX = max(wallA.x,wallB.x);
    float topY = min(wallA.y,wallB.y); float bottomY = max(wallA.y,wallB.y);
    if ( (maxX < leftX-factor) || (minX > rightX+factor) ){return false;}
    if ( (maxY < topY-factor) || (minY > bottomY+factor) ){return false;}
    if ( (rightX-leftX) < 0.01 && (bottomY-topY) <0.01 ){return false;}
    
    Vector3d myDisp = bufferG;
    particle.truePositionLast.displacementToLineSet(wallA,wallB,myDisp);
//    if (myDisp.norm()<.01&&!particle.isPlayer()) println(myDisp.norm());
    myDisp.normalize(); myDisp.multiply(particle.radius);
    float pxin = particle.position.x; float pyin = particle.position.y;
    particle.position.add(myDisp);
    if(!colliding(particle, collisionPoint)){
      particle.position.set(pxin,pyin,0);
      if(checkEdges(particle)){return true;}
      return false;
    }

    particle.position.set(pxin,pyin,0);
    if (particle.isPlayer()) {
//      println("Collision with wall "+this.idNum);
      if (origV == null) origV = new Vector3d(0,0,0);
      origV.set(particle.position); origV.subtract(particle.positionLast);
    }
    wallA.subtract(myDisp); collisionPoint.subtract(myDisp);
    reflectAndFriction(particle.position,wallA,collisionPoint,restCoef,fricCoef);
    reflectAndFriction(particle.positionLast,wallA,collisionPoint,restCoef,fricCoef);
    wallA.add(myDisp); collisionPoint.add(myDisp);
    if (particle.isPlayer()){
      float deltaVSqr = particle.position.minus(particle.positionLast).minus(origV).normSquared();
  //    println(deltaVSqr);
      int numToBurst = min((int) (1.5*deltaVSqr),50);
      float myA = min(numToBurst*255.0/10.0,255);
//      println(numToBurst);
      if (numToBurst > 1) burstFXParticles(numToBurst,particle.position,particle.position.minus(particle.positionLast),particle.myColor,3.0*(myA/255.0),myA);
    }
    return true;
  } 

  public boolean projectCollide(Particle particle, Vector3d collisionPoint){
    //Totally clean of temp vector creation
    
  //This handles a collision by merely projecting the current position rather than altering the velocity.
  //This is crucial if we are implementing iterative constraints - velocity should only be altered once
    if (particle.pinned) return false;
    
    float factor = 2.0*particle.radius;
    float maxX = max(particle.position.x,particle.truePositionLast.x);	
    float maxY = max(particle.position.y,particle.truePositionLast.y);
    float minX = min(particle.position.x,particle.truePositionLast.x);	
    float minY = min(particle.position.y,particle.truePositionLast.y);
    float leftX = min(wallA.x,wallB.x); float rightX = max(wallA.x,wallB.x);
    float topY = min(wallA.y,wallB.y); float bottomY = max(wallA.y,wallB.y);
    if ( (maxX < leftX-factor) || (minX > rightX+factor) ){return false;}
    if ( (maxY < topY-factor) || (minY > bottomY+factor) ){return false;}
    if ( (rightX-leftX) < 0.01 && (bottomY-topY) <0.01 ){return false;}
    
    Vector3d myDisp = bufferG;
    particle.truePositionLast.displacementToLineSet(wallA,wallB,myDisp);
    myDisp.normalize(); myDisp.multiply(particle.radius);
    float pxin = particle.position.x; float pyin = particle.position.y;
    particle.position.add(myDisp);
    if(!colliding(particle, collisionPoint)){
//      particle.position.subtract(myDisp);
      particle.position.set(pxin,pyin,0);
      if(checkEdges(particle)){return true;}
      return false;
    }
//    particle.position.subtract(myDisp);
    particle.position.set(pxin,pyin,0);
    wallA.subtract(myDisp); collisionPoint.subtract(myDisp);
    reflectAndFriction(particle.position,wallA,collisionPoint,0.0,0.0);
    reflectAndFriction(particle.positionLast,wallA,collisionPoint,-1.0,fricCoef);
    wallA.add(myDisp); collisionPoint.add(myDisp);
    return true;    
  }  

  private void reflectAndFriction(Vector3d myPosition, Vector3d lineStart, Vector3d collisionPoint, float perpCoef, float parCoef){
    //8 temp vector creations, not counting any that happen in projectAlong()
/*    Vector3d diff = collisionPoint.minus(lineStart);
    Vector3d diff2 = collisionPoint.minus(myPosition);
    Vector3d proj = diff2.projectAlong(diff);
    Vector3d perp = diff2.minus(proj);
    return myPosition.plus(perp.times(1.0+perpCoef)).plus(proj.times(parCoef));*/
    
    
    //Clean version - no created vectors
    bufferE.set(wallB); bufferE.subtract(lineStart); //use wallB instead of collidePoint in case collidePoint is at position of lineStart
    bufferF.set(collisionPoint); bufferF.subtract(myPosition); //actually should be collisionPoint
    float invMyNorm = 1.0/bufferE.norm(); //BUG - this could be a divide by zero!!!  Better to replace collisionPoint in bufferE with lineEnd, since lineStart->lineEnd is guaranteed != 0 by design [FIXED]
    float bx = bufferE.x * invMyNorm;
    float by = bufferE.y * invMyNorm;
    float thisdotb = bufferF.x*bx+bufferF.y*by;
    bufferE.set(bx*thisdotb,by*thisdotb,0.0);  
    bufferF.subtract(bufferE);
    bufferF.multiply(1.0+perpCoef);
    bufferE.multiply(parCoef);
    bufferE.add(bufferF);
    myPosition.add(bufferE);  
  }
  
  public boolean checkEdges(Particle particle){
      //From lineAthlete codebase, ported from Actionscript
	  float distA = particle.position.distanceTo(wallA);
	  float distB = particle.position.distanceTo(wallB);
	  if (distA > particle.radius && distB > particle.radius){return false;} //no collision
	  else if (distA <= particle.radius){//colliding with wallA endpoint
	    float alpha = particle.radius / (distA+.0001);
	    float oneminusalpha = 1.0-alpha;
  	    particle.position.x = wallA.x*oneminusalpha + alpha*particle.position.x;
            particle.position.y = wallA.y*oneminusalpha + alpha*particle.position.y;
	  } else{//colliding with wallB endpoint
	    float alpha = particle.radius / (distB+.0001);
	    float oneminusalpha = 1.0-alpha;
  	    particle.position.x = wallB.x*oneminusalpha + alpha*particle.position.x;
            particle.position.y = wallB.y*oneminusalpha + alpha*particle.position.y;
	  }
	  return true;
  }

}
